#include <iostream>

#include "app.hpp"
#include "cv_common.hpp"
#include "cv_lidar.hpp"
#include "lidar.hpp"
#include "opencv2/opencv.hpp"
#include "timer.hpp"

#define writer 0

class ObstacleAvoider : private App {
 private:
  Lidar lidar_;
  cv::Mat background;
  component::Recorder recorder_ = component::Recorder("LidarCallerThread");

#if writer
  cv::VideoWriter writer_;
#endif

 public:
  ObstacleAvoider(const std::string log_path) : App(log_path) {
    SPDLOG_WARN("***** Setting Up System. *****");

    lidar_.Open();
    background = cv::imread("../../../../image/background.png");

#if writer
    const int codec = cv::VideoWriter::fourcc('M', 'J', 'P', 'G');
    writer_.open("../../../../image/writer.avi", codec, 60, cv::Size(680, 680));
#endif
  }

  ~ObstacleAvoider() { SPDLOG_WARN("***** Shuted Down System. *****"); }

  void Run() {
    SPDLOG_WARN("******** Running. ********");

    // std::vector<component::LidarPoint> points;

    while (1) {
      auto image = background.clone();
      auto points = lidar_.GetData();
      SPDLOG_DEBUG("size : {}", points.size());
      // component::VisualizeLidarNode(image, points);

#if writer
      writer_.write(image);
#endif

      cv::imshow("show", image);
      if (' ' == cv::waitKey(10)) {
        cv::waitKey(0);
      }

      recorder_.Record();
    }
  }
};

int main(int argc, char const* argv[]) {
  (void)argc;
  (void)argv;

  ObstacleAvoider caller("logs/lidar_caller.log");
  caller.Run();

  return EXIT_SUCCESS;
}
